
#include <stdlib.h>
#include <math.h>

#include "../image.h"

#include "../../util/macros.h"
#include "../../util/misc.h"
#include "../../matrix/matrix.h"
#include "../../matrix/dsp/dsp.h"

static double * _cosr = NULL;
static double * _sinr = NULL;
static double * _hfrac = NULL;
static double * _vfrac = NULL;

static void compute_xy_components(double ray);

static void compute_xy_components(double ray) {

	int i;
	int max_ang;
	double rad;

	if (_cosr != NULL) {
		/* Just computed! */
		return;
	}

	max_ang = 180;

	_cosr = (double *) calloc(max_ang + 1, sizeof (double));
	_sinr = (double *) calloc(max_ang + 1, sizeof (double));
	_hfrac = (double *) calloc(max_ang + 1, sizeof (double));
	_vfrac = (double *) calloc(max_ang + 1, sizeof (double));

	for (i = 0; i < max_ang + 1; i++) {

		rad = deg2rad(i);

		_cosr[i] = ray * cos(rad); // x component
		_sinr[i] = ray * sin(rad); // y component

		_hfrac[i] = _cosr[i] - floor(_cosr[i]); // (x - x1) x difference to linear interpolation
		_vfrac[i] = _sinr[i] - floor(_sinr[i]); // (y - y1) y difference to linear interpolation
	}
}

void image_nonmaxsup_edge(const Matrix * Mag, const Matrix * Or, double ray, Matrix ** NMS) {

	double * _mag;
	double * _or;
	double * _nms;

	int rows;
	int columns;

	int r, c, _r, i;

	double x, y;

	int fx, cx;
	int fy, cy;

	double q11, q21, q12, q22;

	double ti, bi, vi;

	double angle;
	int deg;

	_verify(NMS);
	if (*NMS == NULL) (*NMS) = matrix_new(Mag->rows, Mag->columns);

	_mag = Mag->data;
	_or = Or->data;
	_nms = (*NMS)->data;

	rows = Mag->rows;
	columns = Mag->columns;

	// compute x and y components from ray for each angle [0 - 180]
	compute_xy_components(ray);

	for (r = 0, _r = r * columns; r < rows - 1; r++, _r += columns) {
		for (c = 0; c < columns - 1; c++) {
			i = _r + c;

			/*= Verify the first pixel in gradient direction ================ */
			angle = _or[i];

			/* Convert all angles greater than 180 to the range [0 - 180] to 
			 * avoid invalid index */
			if (angle > M_PI) angle = angle - M_PI;

			angle = rad2deg(angle);

			/* Adjust the right index */
			deg = FIX(angle);

			x = c + _cosr[ deg ];
			y = r + _sinr[ deg ];

			fx = FLOOR(x);
			cx = CEIL(x);
			fy = FLOOR(y);
			cy = CEIL(y);

			/* Avoid invalid indexes */
			if (fx < 0 || fx >= columns) continue;
			if (cx < 0 || cx >= columns) continue;
			if (fy < 0 || fy >= rows) continue;
			if (cy < 0 || cy >= rows) continue;

			q11 = _mag[ fy * columns + fx ];
			q21 = _mag[ fy * columns + cx ];

			q12 = _mag[ cy * columns + fx ];
			q22 = _mag[ cy * columns + cx ];

			bi = q11 + _hfrac[ deg ] * (q21 - q11);
			ti = q12 + _hfrac[ deg ] * (q22 - q12);
			vi = bi + _vfrac[ deg ] * (ti - bi); // Bilinear interpolation

			if (_mag[i] <= vi) continue; // _mag[i] is non-maxima

			/*= Verify the second pixel in opposite gradient direction ====== */
			x = c - _cosr[ deg ];
			y = r - _sinr[ deg ];

			fx = FLOOR(x);
			cx = CEIL(x);
			fy = FLOOR(y);
			cy = CEIL(y);

			/* Avoid invalid indexes */
			if (fx < 0 || fx >= columns) continue;
			if (cx < 0 || cx >= columns) continue;
			if (fy < 0 || fy >= rows) continue;
			if (cy < 0 || cy >= rows) continue;

			q11 = _mag[ fy * columns + fx ];
			q21 = _mag[ fy * columns + cx ];

			q12 = _mag[ cy * columns + fx ];
			q22 = _mag[ cy * columns + cx ];

			bi = q11 + _hfrac[ deg ] * (q21 - q11);
			ti = q12 + _hfrac[ deg ] * (q22 - q12);
			vi = bi + _vfrac[ deg ] * (ti - bi); // Bilinear interpolation

			if (_mag[i] > vi) {
				_nms[i] = _mag[i]; /* _mag[i] is a maxima */
			}

		}
	}

}

void image_nonmaxsup_feature(Matrix * Mat, int ray, Matrix ** NMS) {

	double * mat = NULL;
	double * nms = NULL;

	int r, c, _r, i;
	int mr, mc, _mr, mi;

	int rows;
	int columns;
	long int qtd;

	int d;

	double MAX;

	{ /* Parameters verification ============================================ */
		_verify(Mat);
		_verify(NMS);
		_verify(ray > 0);
	}

	{ /* Initialization ===================================================== */
		mat = Mat->data;
		rows = Mat->rows;
		columns = Mat->columns;
	}

	{ /* Memory Allocation ================================================== */
		if (*NMS == NULL) (*NMS) = matrix_new(rows, columns);
		nms = (*NMS)->data;
	}


	{ /* Verify pixels in SE ================================================ */

		Matrix * SE = matrix_new_ones(1 + ray * 2);
		double * se = SE->data;
		int mrows = SE->rows;
		int mcolumns = SE->columns;

		int rf, cf;

		int mrc = (mrows - 1) / 2;
		int mcc = (mcolumns - 1) / 2;

		int _2columns_minus_2 = (columns << 1) - 2;
		int _2row_minus_2 = (rows << 1) - 2;

		int * r_shift = create_int_sequence(rows, 0, columns);

		qtd = 0;

		// <editor-fold defaultstate="collapsed" desc="CONVOLVE">
		for (r = 0, _r = 0; r < rows; r++, _r += columns) {
			for (c = 0; c < columns; c++) {

				i = _r + c;

				MAX = mat[i];

				for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {

					// reflect y boundaries
					rf = r + (mr - mrc);
					if (rf < 0) rf = -rf;
					else if (rf >= rows) rf = _2row_minus_2 - rf;

					for (mc = 0; mc < mcolumns; mc++) {

						mi = _mr + mc;

						if (!se[mi]) continue;

						// reflect x boundaries
						cf = c + (mc - mcc);
						if (cf < 0) cf = -cf;
						else if (cf >= columns) cf = _2columns_minus_2 - cf;

						d = r_shift[rf] + cf;

						if (mat[d] > MAX) MAX = mat[d];
					}
				}
				if (mat[i] == MAX) {
					nms[i] = MAX;
					qtd++;
				}
			}
		}// </editor-fold>
		free(r_shift);
	}
}

void image_extract_coordinates(Matrix * Mat, double Th, Matrix ** Coord) {

	double * mat;
	double * coord;

	int rows;
	int columns;

	int r, c, _r;
	long int i;
	long int qtd;

	_verify(Mat);
	_verify(Coord);

	mat = Mat->data;
	rows = Mat->rows;
	columns = Mat->columns;

	qtd = 0;

	for (i = 0; i < Mat->size; i++) {
		if (mat[i] > Th) qtd++;
	}

	(*Coord) = matrix_new(qtd, 2);
	coord = (*Coord)->data;

	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {

			i = _r + c;

			if (mat[i] > Th) {
				*coord = c;
				coord++;
				*coord = r;
				coord++;
			}
		}
	}
}

/*
void image_extract_rect(Matrix * Coord, Matrix ** Rect) {

	Matrix * Dist = NULL;
	double * dist = NULL;

	int c1, c2; // Coord index 1 and 2 : first segment
	int c3, c4; // Coord index 3 and 4 : second segment

	{ // Step 01:
		_distance_matrix(Coord, &Dist);
		image_save_matrix_as_image(Dist, "results/dist.pgm");
	}
	{ // Step 02:
		int r, c;
		long int _r, i;

		int rows = Dist->rows;
		int columns = Dist->columns;

		double MAX1;
		double MAX2;


		dist = Dist->data;

		MAX1 = dist[0];
		MAX2 = dist[0];

		for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
			for (c = r; c < columns; c++) {
				i = _r + c;

				if (dist[0] > MAX1) {
					MAX2 = MAX1;
					MAX1 = dist[0];
					c1 = c;
					c2 = r;
					continue;
				}

				if (dist[0] > MAX2) {
					MAX2 = dist[0];
					c3 = c;
					c4 = r;
					continue;
				}

			}
		}
	}









	double * rect;
	rect = (*Rect)->data;

	int x1, y1;
	int x2, y2;
	int x3, y3;
	int x4, y4;

	//x1 =




}

void _distance_matrix(Matrix * Coord, Matrix ** Dist) {

	double * coord;
	double * dist;

	int r, c;
	long int _r, i;

	double x1, y1, x2, y2;
	double x, y;

	int ncoords;

	int rows;
	int columns;

	{
		_verify(Coord);
		_verify(Dist);

		coord = Coord->data;
		ncoords = Coord->rows;

		rows = ncoords;
		columns = ncoords;

		if (*Dist == NULL) (*Dist) = matrix_new(ncoords, ncoords);
		dist = (*Dist)->data;
	}

	{ // Compute distances ==================================================
		for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {

			x1 = coord[ (r << 1) ];
			y1 = coord[ (r << 1) + 1 ];

			for (c = r; c < columns; c++) {
				i = _r + c;

				x2 = coord[ (c << 1) ];
				y2 = coord[ (c << 1) + 1 ];

				x = x1 - x2;
				y = y1 - y2;

				dist[i] = sqrt(x * x + y * y);
			}
		}
	}
}
*/

/**
 *
 * This function realize the nom-maxima suppression of 
 * This function uses bilinear interpolation to 
 *
 *  +===========================================+
 *  |                                           |
 *  |          Bilinear Interpolation           |
 *  |                                           |
 *  |                                           |
 *  |      (0,0)   fx            cx    x        |
 *  |       -|-----|-------------|------>       |
 *  |        |                                  |
 *  |        |                                  |
 *  |        |            x                     |
 *  |        |            |                     |
 *  |        |     q11    bi     q21            |
 *  |     fy -     o------o------o              |
 *  |        |     |      |      |              |
 *  |        |     |      |      |              |
 *  |        |     |      |      |              |
 *  |        |  y --------o      |              |
 *  |        |     |      |      |              |
 *  |        |     |      |      |              |
 *  |        |     |      |      |              |
 *  |     cy -     o------o------o              |
 *  |        |     q12    ti     q22            |
 *  |        |                                  |
 *  |        |                                  |
 *  |     y  |                                  |
 *  |        v                                  |
 *  |                                           |
 *  |     |_________________________________|   |
 *  |                    Image                  |
 *  |                                           |
 *  +-------------------------------------------+
 * 
 * 
 * @param Mag
 * @param Or
 * @param ray
 * @param NMS
 */
/*
void OK_imp_nonmaxsup(const Matrix * Mag, const Matrix * Or, double ray, Matrix ** NMS) {

	double * _mag;
	double * _or;
	double * _nms;

	int rows;
	int columns;

	int r, c, _r, i;

	double x, y;

	int fx, cx;
	int fy, cy;

	double q11, q21, q12, q22;

	double ti, bi, vi;

	double _cosr[180 + 1];
	double _sinr[180 + 1];

	double _hfrac[180 + 1];
	double _vfrac[180 + 1];

	double rad;
	double angle;

	_verify(NMS);
	if (*NMS == NULL) (*NMS) = matrix_new(Mag->rows, Mag->columns);

	_mag = Mag->data;
	_or = Or->data;
	_nms = (*NMS)->data;

	rows = Mag->rows;
	columns = Mag->columns;

	// compute x and y components from ray for each angle [0 - 180]
	for (i = 0; i < 180 + 1; i++) {
		rad = i * M_PI / 180.0;
		_cosr[i] = ray * cos(rad); // x component
		_sinr[i] = ray * sin(rad); // y component

		_hfrac[i] = _cosr[i] - floor(_cosr[i]);
		_vfrac[i] = _sinr[i] - floor(_sinr[i]);
	}

	for (r = 1, _r = columns; r < rows - 1; r++, _r += columns) {
		for (c = 1; c < columns - 1; c++) {
			i = _r + c;

			if (r == 62 && c == 86) {
				matrix_print3x3(Or, c, r);
			}

			//= Verify the first pixel in gradient direction ================ 
			angle = _or[i] * 180 / M_PI;
			if (angle < 0) angle = angle + 180;
			angle = FIX(angle);

			x = c + _cosr[ (int) angle ];
			y = r - _sinr[ (int) angle ];

			//
						fx = FLOOR(x);
						cx = CEIL(x);
						fy = FLOOR(y);
						cy = CEIL(y);
			 
			fx = floor(x);
			cx = ceil(x);
			fy = floor(y);
			cy = ceil(y);


			// Avoid invalid indexes 
			if (fx < 0 || fx > columns) continue;
			if (cx < 0 || cx > columns) continue;
			if (fy < 0 || fy > rows) continue;
			if (cy < 0 || cy > rows) continue;

			q11 = _mag[ fy * columns + fx ];
			q21 = _mag[ fy * columns + cx ];

			q12 = _mag[ cy * columns + fx ];
			q22 = _mag[ cy * columns + cx ];

			//
						bi = q11 + (x - fx) * (q21 - q11);
						ti = q12 + (x - fx) * (q22 - q12);
						vi = bi + (1- (y - fy) ) * (ti - bi); // Bilinear interpolation
			 

			bi = q11 + _hfrac[(int) angle] * (q21 - q11);
			ti = q12 + _hfrac[(int) angle] * (q22 - q12);
			vi = bi + _vfrac[(int) angle] * (ti - bi); // Bilinear interpolation

			if (_mag[i] <= vi) continue; // _mag[i] is non-maxima

			//= Verify the second pixel in opposite gradient direction ====== 
			x = c - _cosr[ (int) angle ];
			y = r + _sinr[ (int) angle ];

			fx = floor(x);
			cx = ceil(x);
			fy = floor(y);
			cy = ceil(y);

			// Avoid invalid indexes 
			if (fx < 0 || fx > columns) continue;
			if (cx < 0 || cx > columns) continue;
			if (fy < 0 || fy > rows) continue;
			if (cy < 0 || cy > rows) continue;

			q11 = _mag[ fy * columns + fx ];
			q21 = _mag[ fy * columns + cx ];

			q12 = _mag[ cy * columns + fx ];
			q22 = _mag[ cy * columns + cx ];

			//
						bi = q11 + (1-(x - fx)) * (q21 - q11);
						ti = q12 + (1-(x - fx)) * (q22 - q12);
						vi = bi + (y - fy) * (ti - bi); // Bilinear interpolation
			 
			bi = q11 + _hfrac[(int) angle] * (q21 - q11);
			ti = q12 + _hfrac[(int) angle] * (q22 - q12);
			vi = bi + _vfrac[(int) angle] * (ti - bi); // Bilinear interpolation

			if (_mag[i] > vi) {
				_nms[i] = _mag[i]; // _mag[i] is a maxima 
			}

		}
	}

}
void image_nonmaxsup_feature_zb(Matrix * Mat, int ray, Matrix ** NMS) {

	Matrix * SE = NULL;

	Matrix * Shift = NULL;
	double * shift;

	double * mat;
	double * nms;
	double * se;

	int rows;
	int columns;
	long int imsize;

	int mrows;
	int mcolumns;

	int r, c, _r, i; // Gray indexes 
	int mr, mc, _mr, mi; // Mask indexes 
	int d;

	double MAX;

	_verify(NMS);
	if (NMS) if (*NMS == NULL) (*NMS) = matrix_new(Mat->rows, Mat->columns);

	SE = matrix_new_ones(1 + ray * 2);
	
	mat = Mat->data;
	nms = (*NMS)->data;
	se = SE->data;

	rows = Mat->rows;
	columns = Mat->columns;
	imsize = Mat->size;

	mrows = SE->rows;
	mcolumns = SE->columns;

	Shift = matrix_create_conv_index_shift(SE->rows, SE->columns, columns);
	shift = Shift->data;

	for (r = 0, _r = 0; r < rows; r++, _r += columns) {

		for (c = 0; c < columns; c++) {

			i = _r + c;

			MAX = mat[i];

			for (mr = 0, _mr = 0; mr < mrows; mr++, _mr += mcolumns) {
				for (mc = 0; mc < mcolumns; mc++) {

					mi = _mr + mc;

					if (!se[mi]) continue;

					d = i + shift[mi];

					// avoid invalid indexes
					if (d < 0 || d >= imsize) continue;

					if (mat[d] > MAX) MAX = mat[d];

				}
			}
			if (mat[i] == MAX) nms[i] = MAX;
			//nms[i] = MAX;
			
			//if (mat[i] == MAX && mat[i] > 600) nms[i] = 255;
			//else nms[i] = 0;
			
		}
	}
	matrix_delete(Shift);
}
 */
